﻿#include"task/Inc/task.h"
#include<QDebug>
void Operation::setImg(Mat &srcUp,Mat &srcBottom)
{
    _src = srcUp;
    _src2 = srcBottom;
}
Operation::Operation()
{
    _arm = make_shared<ArmorDetector>();
    _arm->setEnemyColor(BLUE);
    _buff = make_shared<BuffDetector>();
    _buff2 = make_shared<BuffDetector>();
    _armorOffSet = make_shared<offsetSolve>(_center ,0 ,0);
    _buffOffSet = make_shared<offsetSolve>(_center ,0 ,0);

}
Mat Operation::getMat()
{
    return _src;
}
shared_ptr<ArmorDetector> Operation::getArmorDetector()
{
    return _arm;
}
int ArmorOperation::armorType()
{
    return _arm->getArmorType();
}
shared_ptr<BuffDetector> Operation::getBuffDetectorBottom()
{
    return _buff2;
}
bool ArmorOperation::armCenter() 
{
    _arm->loadImg(_src);
    int findEnemy = _arm->detect();
    if(findEnemy==ArmorDetector::ARMOR_NO)
    {
        return false;
    }
    else
    {
        _center = _arm->getCenterPoint(*_arm);
        return true;
    }
}
shared_ptr<offsetSolve> Operation::getArmorOffSet()
{
    return _armorOffSet;
}
Point2f ArmorOperation::shootCenter()
{
    if(!armCenter())
    {
        return Point2f(0,0);
    }
    else
    {
        Point aimPoint = _center;
        aimPoint  = _armorOffSet->getoffsetArmor(_center);
        return aimPoint;
    }
}
bool BuffOperation::buffCenter()
{
//   if( !_buff->detect(_src))
//    {
//        return false;
//    }
//    else
//    {
//        _center = _buff->getCenter();
//        return true;
//    }
    vector<Point2f> cameraBase;
    vector<Point2f> cameraUp;
    vector<Point2f> srcPoints;
    vector<Point2f> dstPoints;
    Point2f predictPosition;

    Mat homographyMat;
    bool isDetect;
    if(!_src.empty() || !_src2.empty())
    {
        //cout << "--------------------- base detect ----------------------" << endl;
        _buff2->_name = "base";
        isDetect  = _buff2->detect(_src2);
        srcPoints.push_back(_buff2->getPredictCenter());
        if(isDetect)
        {
           cameraBase = _buff2->getBuffPoints();
           //qDebug() << "cameraBase" << cameraBase <<endl;
           _buff2->armorCornerSort(cameraBase);
        }
        else
        {
            return false;
        }
    }
    //cout << "--------------------- up detect ----------------------"<<endl;
    if( _buff->detect(_src))
    {
       _buff->_name = "upside";
       cameraUp = _buff->getBuffPoints();
       //cout << "cameraUp" << cameraUp << endl;
       dstAngle = _buff->armorCornerSort(cameraUp) +_buff->_increaseAngle*360/2.0/3.1415;
    // int dstAngle = detect.armorCornerSort(cameraUp) ;
       dstAngle %=360;
       if(cameraUp.size()==4 && cameraBase.size()==4)
       {
            homographyMat = findHomography(cameraBase,cameraUp,0);
            //cout << "Homography Matrix is : \n"<< homographyMat << endl;
            if(!homographyMat.empty())
                perspectiveTransform(srcPoints, dstPoints, homographyMat);
            predictPosition=dstPoints[0];
            if(dstPoints[0].x !=0 && dstPoints[0].y !=0 && predictPosition.x!=0 && predictPosition.y!=0)
            {
                //cout << "dstPiont" << dstPoints[0] << endl;
                if (dstPoints[0].x >0 && dstPoints[0].y>0)
                {
                    circle(_src,dstPoints[0],5,Scalar(255, 255, 0),3);
                }
                putText( _src,"angle: "+to_string(dstAngle), Point(10,80),FONT_HERSHEY_SIMPLEX,0.5, Scalar (0,255,0),2);
                _center = dstPoints[0];
                return true;
            }
            else
            {
                return false;
            }
       }
       else
       {
           return false;
       }
    }
}
shared_ptr<offsetSolve> Operation::getBuffOffSet()
{
    return _buffOffSet;
}
Point2f BuffOperation::shootCenter()
{
    if(!buffCenter())
    {
        return Point2f(0,0);
    }
    else
    {
        int offsetX[8]={23,//右
                        18,//右上
                        18,//上
                        13,//左上
                        23,//左
                        23,//左下
                        15,//下
                        18};//右下
        int offsetY[8]={105,//右
                        115,//右上
                        120,//上
                        110,//左上
                        100,//左
                        100,//左下
                        100,//下
                        105};//右下
        int dx = offsetX[dstAngle/45]+(offsetX[(dstAngle/45+1)%8]-offsetX[dstAngle/45])*(dstAngle%45)/45.0;
        int dy = offsetY[dstAngle/45]+(offsetY[(dstAngle/45+1)%8]-offsetY[dstAngle/45])*(dstAngle%45)/45.0;
        putText(_src,"dx: "+to_string(dx), Point(10,100),FONT_HERSHEY_SIMPLEX,0.5, Scalar (0,255,0),2);
        putText(_src,"dy: "+to_string(dy), Point(10,120),FONT_HERSHEY_SIMPLEX,0.5, Scalar (0,255,0),2);
        Point aimPoint;
//        _buffOffSet->setOffSetX(dx);
//        _buffOffSet->setOffSetY(dy);
        aimPoint  = _buffOffSet->getoffsetBuff(_center);
        aimPoint.x += dx;
        aimPoint.y -= dy;
        circle(_src,aimPoint,5,Scalar(0, 255, 0),3);
        return aimPoint;
    }
}

Point2f NoTaskOperation::shootCenter()
{
    return Point2f(320,240);
}


shared_ptr<Operation> ArmorShootFactory::creatTask()
{
    return shared_ptr<ArmorOperation>(new ArmorOperation());
}

shared_ptr<Operation> BuffShootFactory::creatTask()
{
    return shared_ptr<BuffOperation>(new BuffOperation());
}

shared_ptr<Operation>  NoTaskFactory::creatTask()
{
    return shared_ptr<NoTaskOperation>(new NoTaskOperation());
}
